#ifndef IM_JOINT_H
#define IM_JOINT_H

#include <urdf/model.h>
#include <sensor_msgs/JointState.h>
#include <interactive_markers/interactive_marker_server.h>
#include <tinyxml.h>
#include <string.h>
#include <boost/lexical_cast.hpp>
#include <tf/tf.h>
#include <math.h>

namespace im_joint_state_publisher {

class ImJoint {
public:
	ImJoint(
			boost::shared_ptr<const urdf::Joint> joint,
			interactive_markers::InteractiveMarkerServer* server,
			double,
			interactive_markers::InteractiveMarkerServer::FeedbackCallback callBack);

	//// properties of joint
	std::string joint_name;
	std::string parent_name;
	std::string mimic_name;
	tf::Vector3 origin;
	tf::Quaternion rotation;
	double joint_angle;
	double multiplier, offset;
	double scale;
	enum {
		REVOLUTE, CONTINUOUS, PRISMATIC, FIXED
	} type;

	//// function to maniputate joint
	void setJointRotation(
			const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
private:
	double upper_limit;
	double lower_limit;
	geometry_msgs::Point joint_axis;

	void makeFrame(
			interactive_markers::InteractiveMarkerServer* server,
			interactive_markers::InteractiveMarkerServer::FeedbackCallback callBack);
	void add_rotation(visualization_msgs::InteractiveMarker *frame_marker);
	void add_translation(visualization_msgs::InteractiveMarker *frame_marker);
};

}
// namespace im_joint_state_publisher
#endif
